#include "lscm_fsd_lidar.h"
#include "lscm_usart_2.h"



extern UART_HandleTypeDef huart1;

uint8_t tx_buffer[USART_TX_BUF_SIZE];

uint8_t rx_data;
uint8_t Lidar_Raw_Buffer[CIRCULAR_BUF_COUNT][RX_BUF_SIZE];
uint16_t Raw_Data_index=0;
uint8_t Circular_Buffer_Tail=0;
uint8_t Circular_Buffer_Head=0;
uint8_t Circular_Buffer_Overflow=0;
__IO int Obstacle[360]={0};
__IO MovingAveNode movingaveNode[360];
//int movingavecount[5]


void FSD_Lidar_Init(void)
{
  
    lscm_Lidar_reset_buffer(CIRCULAR_BUF_COUNT);
    HAL_UART_Receive_DMA(&huart1, &rx_data, USART1_DMA_IRQ_TRI);
   

}

//open the lidar by sending "#sf\r\n"
void lscm_LIDAR_Open (void)
{

    HAL_UART_Transmit_DMA(&huart1, (uint8_t *)LIDAR_OPEN_CMD, 5);
}


//Stop Lidar
void lscm_LIDAR_Close (void)
{

    HAL_UART_Transmit_DMA(&huart1, (uint8_t *)LIDAR_STOP_CMD, 7);
}



//Clear Buffer received from Lidar 

void lscm_Lidar_reset_buffer (int index)
{
	if(index<CIRCULAR_BUF_COUNT)
	{
		 for (int i=0; i<RX_BUF_SIZE; i++)
    {
        Lidar_Raw_Buffer[index][i] = 0x00;
    }
		
	
	}
	else
	{
		for(int y=0;y<CIRCULAR_BUF_COUNT;y++)
			for (int i=0; i<RX_BUF_SIZE; i++)
			{
					Lidar_Raw_Buffer[y][i] = 0x00;
			}
    Raw_Data_index = 0;
	
	
	}

    
}
// Store data
void lscm_lidar_enqueue_data_to_buffer (uint8_t data)
{

	Lidar_Raw_Buffer[Circular_Buffer_Tail][Raw_Data_index++]=data;
	if(Raw_Data_index==RX_BUF_SIZE)
	{
		Circular_Buffer_Tail=(Circular_Buffer_Tail+1)%CIRCULAR_BUF_COUNT;
		
		if(Circular_Buffer_Tail==0)
		  Circular_Buffer_Overflow++;
		
		Raw_Data_index=0;
	
	}
	HAL_UART_Receive_DMA(&huart1, &rx_data, USART1_DMA_IRQ_TRI);

}

//Decode a circular buffer to get the distance of an object in a specific degree ranging from 0 to 359
void lscm_lidar_decode_buffer(void)
{
	uint8_t temp_tail_count;
	uint8_t data_start_index=0;
	
	//If the circular buffer head is lower than the tail, read,otherwise wait.
	 temp_tail_count=Circular_Buffer_Overflow*CIRCULAR_BUF_COUNT+Circular_Buffer_Tail;
	if(Circular_Buffer_Head<temp_tail_count)
	{
		//Find the start flag of  a bunch of receiving buffer
		data_start_index=GetDataStartflag(Lidar_Raw_Buffer[Circular_Buffer_Head]);
		
		FSDNode node;
		for(int i=data_start_index;i<RX_BUF_SIZE;i+=DATA_OFFSET)
		{
			uint8_t temp_buf[4];
			for(int j=0;j<DATA_OFFSET;j++)
			{
				temp_buf[j]=Lidar_Raw_Buffer[Circular_Buffer_Head][i+j];
			
			}
			
			//Decode buffer and get a node 
			node=GetFSDnode(temp_buf);
			
			//  if the node is available, pass it to moving average filter
			if(node.error==0)
			{
				
				Obstacle[node.Angle]=MovingAverageFilter(node);
			  
			
			}

			//lscm_usart2_printf("Angle: %d,Distance: %d\r\n",node.Angle,node.distance);
		
		}
		//Clear Buffer that we have read
		lscm_Lidar_reset_buffer(Circular_Buffer_Head);
		
		//Update Circular buffer
		Circular_Buffer_Head=(Circular_Buffer_Head+1)%CIRCULAR_BUF_COUNT;
		if(Circular_Buffer_Head==0)
			Circular_Buffer_Overflow--;
	
	}
	


}

//Filter to get a stable distance 
int MovingAverageFilter(FSDNode node)
{
	  int distance_temp=0;
		movingaveNode[node.Angle].distance[movingaveNode[node.Angle].count++]=node.distance;
		if(movingaveNode[node.Angle].count==MOVINGAVE_COUNT)
		{
			
			
			for(int i=0;i<(MOVINGAVE_COUNT-1);i++)
			{
				distance_temp+=movingaveNode[node.Angle].distance[i];
				movingaveNode[node.Angle].distance[i]=movingaveNode[node.Angle].distance[i+1];
				
			}
			distance_temp+=movingaveNode[node.Angle].distance[MOVINGAVE_COUNT-1];
			movingaveNode[node.Angle].count--;
			
			
			distance_temp/=MOVINGAVE_COUNT;
		
		}
		
		return distance_temp;

}

unsigned char GetCrcPackage(unsigned char *buf)
{
	return (cbit[buf[1]]+cbit[buf[2]]+cbit[buf[3]])&0x07;
}



FSDNode GetFSDnode(uint8_t *buf)
{
	FSDNode fsdNode;
	unsigned char crcdata = GetCrcPackage(buf);
	unsigned char crcdata1 = (buf[0]>>4)&0x07;
	if(crcdata1!= crcdata)
	  fsdNode.error=1;
	else
	{
		//fsdNode.error=0;
		unsigned int ustemp;

		//compute distance
		ustemp= (buf[0]&0x0F);
		ustemp<<= 7;
		ustemp+= (buf[1]&0x7F);
		ustemp<<= 1;
		if( buf[2]&0x40)
				ustemp++;

		fsdNode.distance = ustemp;

		ustemp= buf[2]&0x3F;
		ustemp<<= 7;
		ustemp+= (buf[3]&0x7F);
		fsdNode.Angle = ustemp/16;
		
		if(ustemp>=5760||fsdNode.distance==0)
			fsdNode.error=1;
		else
			fsdNode.error=0;
		
//		if(fsdNode.Angle==1)
//		lscm_usart2_printf("Error: %d,Angle: %d,Distance: %d\r\n",fsdNode.error,fsdNode.Angle,fsdNode.distance);

	}
		//return 1;
	
	return fsdNode;


}

// For every bunch of data, the 7th bit of buf[0],buf[1],buf[2] is 0,
// while that of buf[3] is 1.

uint8_t Data_Structure_Validation(unsigned char *buf)
{

	if ( ((*((int *)buf))&0x80808080) != 0x80000000) 
		return 0;
	else
		return 1;

}

// A bunch of data consists of 4 character
uint8_t GetDataStartflag(uint8_t *Data)
{
	uint8_t flag;
	unsigned char buf[DATA_OFFSET];
	for(int i=0;i<RX_BUF_SIZE;i++)
	{
		for(uint8_t j=0;j<DATA_OFFSET;j++)
			buf[j]=Data[i+j];
		
		if(Data_Structure_Validation((unsigned char*)buf)==1)
		{
			flag=i;
			break;
		
		}
	}
	
	return flag;
	
}

void ShowAllObstacle(void)
{
	for(int i=0;i<20;i++)
	{
		lscm_usart2_printf("Degree: %d,Distance: %d\r\n",i,Obstacle[i]);
	
	}


}

void DisplayOverFlowNum(void)
{
	lscm_usart2_printf("Over flow Num: %d\r\n",Circular_Buffer_Overflow);

}
